from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node, LifecycleNode
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    package_dir = get_package_share_directory('fangshe_sm')
    default_params_file = PathJoinSubstitution([
        package_dir, 'config', 'state_machine.params.yaml'
    ])

    # Declare launch arguments
    params_file_arg = DeclareLaunchArgument(
        'master_params_file',
        default_value=default_params_file,
        description='Path to the master YAML file for the state machine and sub-nodes'
    )

    # Get parameters file path
    params_file = LaunchConfiguration('master_params_file')

    # Start sub-nodes (LifecycleNodes) - They start in 'unconfigured' state
    center_detection_node = LifecycleNode(
        package='center_detection',
        executable='center_detector', # Replace with actual executable name
        name='center_detector',
        namespace='',
        parameters=[{'use_sim_time': False}], # Can pass initial parameters if needed
        output='screen'
    )

    local_navigation_node = LifecycleNode(
        package='local_navigation',
        executable='local_navigator', # Replace with actual executable name
        name='local_navigator',
        namespace='',
        parameters=[{'use_sim_time': False}],
        output='screen'
    )

    mqtt_cloud_sub_node = LifecycleNode(
        package='mqtt_cloud_sub',
        executable='cloud_subscriber', # Replace with actual executable name
        name='pointcloud_mqtt_subscriber',
        namespace='',
        parameters=[{'use_sim_time': False}],
        output='screen'
    )

    line_alignment_node = LifecycleNode(
        package='line_alignment',
        executable='line_aligner', # Replace with actual executable name
        name='line_aligner',
        namespace='',
        parameters=[{'use_sim_time': False}],
        output='screen'
    )

    gripper_control_node = LifecycleNode(
        package='gripper_control',
        executable='gripper_communicator', # Replace with actual executable name
        name='gripper_communicator',
        namespace='',
        parameters=[{'use_sim_time': False}],
        output='screen'
    )

    # Start the master state machine node (Regular Node)
    master_state_machine_node = Node(
        package='fangshe_sm',
        executable='state_machine', # Matches the executable name in CMakeLists.txt
        name='state_machine',
        namespace='',
        parameters=[params_file], # Pass the path to the master config file
        output='screen'
    )

    return LaunchDescription([
        params_file_arg,
        # Launch all sub-nodes first
        center_detection_node,
        local_navigation_node,
        mqtt_cloud_sub_node,
        line_alignment_node,
        gripper_control_node,
        # Launch the master state machine last
        master_state_machine_node,
    ])